clc
clear

arfa=36.795/180*pi;
beta=78.169/180*pi;


%��һ���Ƚ�������任��Ȼ�������µ�����ϵ�п������ǵ�����
T1=[cos(pi/2-beta)      0   -sin(pi/2-beta)      0 
                   0                 1                0                0
        sin(pi/2-beta)       0     cos(pi/2-beta)      0
                0                    0                    0             1];


ttt=0;
for i=1:100
    ttt=ttt+1;
end
kkk=0;
for i=1:100
    kkk=kkk+1;
end            
            
T2=[cos(arfa)       sin(arfa)         0         0
          -sin(arfa)     cos(arfa)        0         0
                 0            0                    1         0
                0             0                     0        1];

[D1,S1 ]=xlsread('����1.csv','����1','A2:D2227'); %��ȡ��׼�������ڵ�����
[D2,S2]=xlsread('����2.csv','����2','A2:G2227'); %��ȡ�ٶ������¶˵��׼̬ʱ�˵�����



for i=1:length(D1)
   DD1(i,:)=[D1(i,:)       1]*inv(T1*T2);    %���º�Ļ�׼�������ڵ�����
   DD2(i,:)=[D2(i,1:3)   1]*inv(T1*T2);    %���º�Ĵٶ����¶˵��׼̬ʱ�˵�����
   DD3(i,:)=[D2(i,4:6)   1]*inv(T1*T2);    %���º�Ĵٶ����϶˵��׼̬ʱ�˵�����
end
%Ϊ�˱��ں���ʹ������1���㷨���򣬽����������µ���Ϊԭʼ����ϵ�µı�������
D1=DD1(:,1:3);
D2=[DD2(:,1:3)  DD3(:,1:3)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%������������һ�ķ����������
%���¾�Ϊ������ϵ�µļ������
plot3(D1(:,1),D1(:,2),D1(:,3),'b.')  %��׼�������ڵ�����
hold on
for i=1:2226
      plot3([D2(i,1) D2(i,4) ],[D2(i,2) D2(i,5)],[D2(i,3) D2(i,6)],'g') %���ƴٶ������¶˵�����
      plot3([D2(i,4) D1(i,1) ],[D2(i,5) D1(i,2)],[D2(i,6) D1(i,3)],'y')  %�ٶ����϶˵㵽��׼�������ڵ�
end
X=mean(D1(:,1));
Y=mean(D1(:,2));
Z=mean(D1(:,3));
X2=mean(D2(:,1));
Y2=mean(D2(:,2));
Z2=mean(D2(:,3));
X3=mean(D2(:,4));
Y3=mean(D2(:,5));
Z3=mean(D2(:,6));
plot3([0 X],[0 Y],[0 Z],'r');
%������������
R=300.4;
f=281.662;
c=300.799;
theta=0:0.01:2*pi;  %�����߼�����Ƕȷ�Χ
r=repmat(0:0.5:150,length(theta),1);   %�����߼�����뾶��Χ  ����Ϊ������ʽ
theta=repmat(theta',1,size(r,2));   %�����߼�����Ƕȷ�Χ ���µ����Ƕ�Ϊ������ʽ
x=r.*cos(theta);  %������ת��Ϊֱ����
y=r.*sin(theta);   %������ת��Ϊֱ����
z=(x.^2+y.^2)/(2*f)-c;  %���������߷���
surf(x,y,z)
shading interp
view([1 0 0])
axis equal
%%%%%%%%%%%%%%%%%%���ٶ������¶˵��������������潻��
M=D1(:,1)-D2(:,1);  %MNPΪ�ռ�ֱ�߱�׼�����м����
N=D1(:,2)-D2(:,2);
P=D1(:,3)-D2(:,3);
k=0; %�ھ�С�ڵ���300�׵ķ�Χ�������ڵ����
DD=zeros(size(D1)); %��ʼ����¼��Ӧ��i�������ڵ�仯���λ������
rc=[];
for i=1:length(D1(:,1))
    if (D1(i,1).^2+D1(i,2).^2).^0.5<=150&(D1(i,1).^2+D1(i,2).^2).^0.5>=0  %����ھ�С�ڵ���300�׵ķ�ΧҪ��
        rc=[rc i];  %��¼��300�׿ھ��ڵĽڵ����
        k=k+1;
    x0=D2(i,1); %�ٶ����¶˵�
    y0=D2(i,2);
    z0=D2(i,3);
    m=M(i);  %����Ϊֱ�߷���xyz����ϵ��
    n=N(i);
    p=P(i);
    if m^2+n^2>0   %����ٶ�������ֱ��������������Ľ���
       t=qiugen(x0,y0,z0,m,n,p,f,c);
       zz(k)=min(z0+p*t); %ɸѡ��С��0��Z���꽻��
       t=(zz(k)-z0)/p;
       xx(k)=x0+m*t;
       yy(k)=y0+n*t;
       DD(i,:)=[xx(k) yy(k)  zz(k)];%��¼��Ӧ��i�������ڵ�仯���λ��
    else   %����ٶ�������ֱ����Z��ƽ��
       xx(k)=x0;
       yy(k)=y0;
        zz(k)=(x0.^2+y0.^2)/(2*f)-c;
      DD(i,:)=[xx(k) yy(k)  zz(k)];%��¼��Ӧ��i�������ڵ�仯���λ��
    end
    else
    DD(i,:)=[inf inf inf]; %����300�׿ھ���Χ�ڵ����¼Ϊ��Чֵ
   end
end
plot3(xx,yy,zz,'r.') %���Ƴ����дٶ�������ֱ��������������Ľ���
alpha(0.5)

%%%%%%%%%%����ٶ�������ֱ��������������Ľ��� �� ��׼����ı仯����
for i=1:length(D1)
    distance(i)=(sum((DD(i,:)-D1(i,:)).^2))^0.5; %ֱ�Ӽ�����仯ǰ��ľ���
end
distance(find(isinf(distance)))=0;  %����Ч�����滻Ϊ0
N=length(find(distance>0.6))  %��ʾ��������������Ƶ������ڵ����



ttt=0;
for i=1:100
    ttt=ttt+1;
end
kkk=0;
for i=1:100
    kkk=kkk+1;
end
F=[0  0  f/2-c];
for i=1:length(DD(:,1)) %���μ����i�������ڵ�仯���뷽Բ1��СԲ��Ľ���
    %����P���Z������ֵ��仯��ĵ�i�������ڵ��뽹������ֱ�߷���
    %P���Z������ֵ
    PZ=R*0.466-R; %��ֵ
    %�����ڵ��뽹�㷽������
    mx=DD(i,1);
    ny=DD(i,2);
    pz=DD(i,3)-(f/2-c);
    t=(PZ-(f/2-c))/pz; %����P�����������ֱ�߲��������м����
    PX(i)=mx*t;       %�õ���i�������ڵ�仯���뷽Բ1��СԲ��Ľ���
    PY(i)=ny*t;
    plot3([DD(i,1)  0],[DD(i,2)  0],[DD(i,3)  f/2-c],'b')  %���������ڵ��뽹������
end
figure(2) %���ƻ��������ڵ��뽹��������СԲ�潻��ֲ�ͼ
hold on
for  i=1:length(DD(:,1))
    plot3(PX(i),PY(i),PZ,'k.')  
end
theta=0:0.01:2*pi;  %P��СԲ�漫����Ƕȷ�Χ
r=repmat(0:0.05:1,length(theta),1);   %СԲ�漫����뾶��Χ  ����Ϊ������ʽ
theta=repmat(theta',1,size(r,2));   %СԲ��v������Ƕȷ�Χ ���µ����Ƕ�Ϊ������ʽ
x=r.*cos(theta);  %������ת��Ϊֱ����
y=r.*sin(theta);   %������ת��Ϊֱ����
z=PZ*ones(size(theta));  %СԲ��������
surf(x,y,z)
shading interp
alpha(0.5)
view([0  0  1])
save  rc  rc  DD



%���Ӿ���仯������
load LL    %�������������ڵ����ӹ�ϵ����
LF=zeros(size(LL));  %������ʼ�жϾ���
for i=1:length(LL)    %��Ӧ������
    for j=1:size(LL,2)   %��Ӧ����
          for k=1:length(rc)     %��Ӧ300�׿ھ��ڵĵ�k���ڵ�
               if  LL(i,j)==rc(k)     %�����k���ڵ������ھ���LL��i��j����ͬ����LFȡ1
                   LF(i,j)=1;
               end
          end
    end
end
lf=sum(LF') ;  %�����Ϊ2���ʾ�����Ӵ���
RLL=LL(find(lf==2),:); %300�׿ھ��ڵ��������ӹ�ϵ����
n=length(RLL)  %����Ч���ӽ��м���



for i=1:length(RLL)
     %�������ǰ��i�����ӵĳ���
     d1(i)=((D1(RLL(i,1),1)-D1(RLL(i,2),1))^2+(D1(RLL(i,1),2)-D1(RLL(i,2),2))^2+(D1(RLL(i,1),3)-D1(RLL(i,2),3))^2)^0.5;
     %����������i�����ӵĳ���
     d2(i)=((DD(RLL(i,1),1)-DD(RLL(i,2),1))^2+(DD(RLL(i,1),2)-DD(RLL(i,2),2))^2+(DD(RLL(i,1),3)-DD(RLL(i,2),3))^2)^0.5;
     %�������ǰ��仯��
     ra(i)=abs(d2(i)-d1(i))/d1(i);
end

ttt=0;
for i=1:100
    ttt=ttt+1;
end


kkk=0;
for i=1:100
    kkk=kkk+1;
end

nl=length(find(ra>0.0007));  %���������ӳ��ȱ仯Ҫ�������
NLL=RLL(find(ra>0.0007),:); %���������ӳ��ȱ仯�����Ӽ���
GLL=RLL(find(ra<=0.0007),:); %�������ӳ��ȱ仯�����Ӽ���
%��ͼ�������ӹ�ϵ
figure
hold on
for i=1:length(RLL)    %����300�׿ھ��ڵ������߶�
     plot3([D1(RLL(i,1),1)   D1(RLL(i,2),1)],[D1(RLL(i,1),2)   D1(RLL(i,2),2)],[D1(RLL(i,1),3)   D1(RLL(i,2),3)],'b') 
end
for i=1:length(NLL)    %���Ʋ��������ӵ���Ҫ��������߶�
     plot3([D1(NLL(i,1),1)   D1(NLL(i,2),1)],[D1(NLL(i,1),2)   D1(NLL(i,2),2)],[D1(NLL(i,1),3)   D1(NLL(i,2),3)],'r') 
end
%%%%%%%%%%%%%%%%%%%%%%��任�õ����������涥������
Q=[0  0  -c 1]*T1*T2  %���Ϊ[-49.3855  -36.9384 -294.4090]
for i=1:length(D1)
   DT1(i,:)=[DD(i,:)   1]*T1*T2;    %��任��������ڵ�仯�������
end
DT=DT1(:,1:3);
%������任����ԭ����ϵ�е���ǰ�����ڵ�ı仯����
[D1 S1 ]=xlsread('����1.csv','����1','A2:D2227'); %��ȡ��׼�������ڵ�����
for i=1:length(DT)
    td(i)=norm(DT(i,:)-D1(i,:)); %����仯����
    if norm(DT(i,:))>norm(D1(i,:))
        td(i)=-td(i);
     end
end
xlswrite('DT.xls',DT)
xlswrite('td.xls',td')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%����ʵ�ʽ��ձ�
ttt=0;
for i=1:100
    ttt=ttt+1;
end


kkk=0;
for i=1:100
    kkk=kkk+1;
end


load LLL %��������������ߵ����Ӽ���
FFF=zeros(length(LLL) ,3);
for i=1:length(LLL)  %�����ж�ÿ�����������������Ƿ��ڼ���GLL��
    for j=1:3
         for k=1:length(GLL)
               if norm(LLL(i,(2*j-1):(2*j))-GLL(k,:))==0 %ȡֵΪ0����ڸ�����
                   FFF(i,j)=1;
               end
         end
    end
end
sf=sum(FFF');
gn=length(find(sf==3));
%4 ������300�׿ھ������з�����������GN;(������ΪRLL)����߱�ֵΪʵ�ʽ��ձȡ�
FFF2=zeros(length(LLL) ,3);
for i=1:length(LLL)  %�����ж�ÿ�����������������Ƿ��ڼ���GLL��
    for j=1:3
         for k=1:length(RLL)
               if norm(LLL(i,(2*j-1):(2*j))-RLL(k,:))==0 %ȡֵΪ0����ڸ�����
                   FFF2(i,j)=1;
               end
         end
    end
end
sf2=sum(FFF2');
GNs=length(find(sf2==3));
%���ձ�Ϊ
rates=gn/GNs;  %����ֵΪ0.7792


clc
clear
theta=0:0.001:0.2; %����������Ϊ����������������һ���������Y��ļн�Ϊtheta�����侭��P���ֱ�����Ϊpi/2��ȥ2*theta
R=300.4;
x0=-R*sin(theta);  %�����ϵ������
y0=-R*cos(theta);
x=((0.466-1)*R-y0)./tan(pi/2-2*theta)+x0;  %��������P��СԲ�潻���x����
plot(theta,x)  %����theta�ı仯����x�仯����
c=find(abs(x)<0.5);  %�ҳ�����СԲ��ĵ����
hold on
plot(theta(c),x(c),'r.') ; %�鿴theta�Ķ�Ӧ����
theta1=max(theta(c)) ;        %�ҳ�����Y����������theta��
clear theta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%�ҳ�����Y���Զ�����theta������
theta=0.2:0.001:pi/6;
R=300;
x0=-R*sin(theta);
y0=-R*cos(theta);
x=((0.466-1)*R-y0)./tan(pi/2-2*theta)+x0;
plot(theta,x)
c=find(abs(x)<0.5);
hold on
plot(theta(c),x(c),'r.')
[min((theta(c)))  max(theta(c)) ];
maxtheta=max(theta(c));
mintheta=min((theta(c)));
%�����������������Ƕ�Ӧ���������������2*pi*R*h��ʽ R�Ǵ�Բ�뾶 h����Բ�߶�
h1=R-R*cos(theta1);
S1=2*pi*R*h1;  %����Y�������������
hmax=(R-R*cos(maxtheta));
hmin=(R-R*cos(mintheta));
S2=2*pi*R*(hmax-hmin);  %Զ��Y�������������
%300�׿ھ���Ӧ��������
h=R-R*cos(pi/6);
S=2*pi*R*h;
%��������� ���ձ�Ϊ
rate=(S1+S2)/S









